#!/usr/bin/python3
# coding=utf8

import rospy
import tf.transformations
from geometry_msgs.msg import Pose, Point, Quaternion, PoseWithCovarianceStamped

if __name__ == "__main__":
    rospy.init_node("publish_initial_pose")
    pub_pose = rospy.Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=1)

    xyz = [0, 0, 0]
    roll,pitch,yaw = [0, 0, 0]
    quat = tf.transformations.quaternion_from_euler(roll,pitch,yaw)

    initial_pose = PoseWithCovarianceStamped()
    initial_pose.pose.pose = Pose(Point(*xyz), Quaternion(*quat))
    initial_pose.header.stamp = rospy.Time().now()
    initial_pose.header.frame_id = "map"
    euler = tf.transformations.euler_from_quaternion(quat)
    rospy.sleep(2)
    rospy.loginfo("----------------------------------------")
    rospy.loginfo("POSE INIT !")
    rospy.loginfo("Initial Position: x={}, y={}, z={}".format(xyz[0], xyz[1], xyz[2]))
    rospy.loginfo(
        "Initial Orientation (euler): roll={:.3f}, pitch={:.3f}, yaw={:.3f}".format(
            *euler
        )
    )
    rospy.loginfo("----------------------------------------")
    pub_pose.publish(initial_pose)
